#include <ros/ros.h>
#include "../sensor_handle/lidar_undistortion_2d.h"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

class TestLidarUndistortion{
    public:
        TestLidarUndistortion();
        ~TestLidarUndistortion();

        // 激光雷达回调函数
        void LaserScanCallback(const sensor_msgs::LaserScanConstPtr& msg);
        // IMU回调函数
        void ImuCallback(const sensor_msgs::ImuConstPtr& msg);
        // 将 LaserScan 转换为 PointCloud2 的函数
        void laserScanToPointCloud2(const sensor_msgs::LaserScan& scan, sensor_msgs::PointCloud2& cloud);
        
    private:
        ros::NodeHandle __nh;
        ros::Subscriber __imu_sub, __scan_sub;
        ros::Publisher __scan_undistortion_pub;
        ros::Publisher __scan_undistortion_cloud_pub;

        Lidar2DUndistortion __lidar_2d_undistortion;
};


TestLidarUndistortion::TestLidarUndistortion(){
    __scan_undistortion_pub = __nh.advertise<sensor_msgs::LaserScan>("/scan_undistortion", 1);
    __scan_undistortion_cloud_pub = __nh.advertise<sensor_msgs::PointCloud2> ("/scan_undistortion_cloud_pub", 10);
    __scan_sub = __nh.subscribe("/scan_raw", 10, &TestLidarUndistortion::LaserScanCallback, this);
    __imu_sub = __nh.subscribe("/imu_raw", 10, &TestLidarUndistortion::ImuCallback, this);
}

TestLidarUndistortion::~TestLidarUndistortion(){
    
}

// IMU回调函数
void TestLidarUndistortion::ImuCallback(const sensor_msgs::ImuConstPtr& msg){
    __lidar_2d_undistortion.PushImuData(msg);
}

// 激光雷达回调函数
void TestLidarUndistortion::LaserScanCallback(const sensor_msgs::LaserScanConstPtr& msg){
    sensor_msgs::LaserScan scan_undistortion_msg;
    __lidar_2d_undistortion.HandleLidarScanData(msg, scan_undistortion_msg);
    __scan_undistortion_pub.publish(scan_undistortion_msg);

    sensor_msgs::PointCloud2 cloud_msg;
    laserScanToPointCloud2(scan_undistortion_msg, cloud_msg);
    __scan_undistortion_cloud_pub.publish(cloud_msg);
}

// 将 LaserScan 转换为 PointCloud2 的函数
void TestLidarUndistortion::laserScanToPointCloud2(const sensor_msgs::LaserScan& scan, sensor_msgs::PointCloud2& cloud) {
    // 初始化 PointCloud2 消息
    cloud.header = scan.header;  // 保持相同的 header
    cloud.height = 1;            // 单行点云
    cloud.width = scan.ranges.size();
    cloud.is_bigendian = false;
    cloud.is_dense = false;      // 是否有无效数据（NaN）

    // 设置 PointCloud2 字段
    sensor_msgs::PointCloud2Modifier modifier(cloud);
    modifier.setPointCloud2FieldsByString(1, "xyz");

    // 使用迭代器填充数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");

    // 遍历激光雷达的所有测量点
    for (size_t i = 0; i < scan.ranges.size(); ++i) {
        float range = scan.ranges[i];
        if (range >= scan.range_min && range <= scan.range_max) {
            // 计算对应的角度
            float angle = scan.angle_min + i * scan.angle_increment;

            // 极坐标转换为笛卡尔坐标
            *iter_x = range * cos(angle);  // x 坐标
            *iter_y = range * sin(angle);  // y 坐标
            *iter_z = 0.0;                 // 激光雷达数据为2D, z 设为0

            ++iter_x;
            ++iter_y;
            ++iter_z;
        }
    }
}

/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "test_lidar_2d_undistortion");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>

    ROS_INFO("test_lidar_2d_undistortion initializating......");
    ros::NodeHandle nh;
    TestLidarUndistortion test_lidar_2d_undistortion;
    ros::spin();
    return 0;
}

